/** Interactive Surface  FYP- 25 Interactive Surface FYP- 25 Interactive Surface FYP- 25
 *
 *  @author Acer
 */
package utilities;

import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_video;
import com.googlecode.javacv.cpp.opencv_video.CvKalman;
import java.awt.Color;
import java.awt.event.MouseEvent;
import java.util.Random;
import javax.swing.JFrame;
import test.shashindra.ShashKalmanTest;

import com.googlecode.javacv.cpp.opencv_video.CvKalman;
import com.googlecode.javacv.cpp.opencv_calib3d.*;
import static com.googlecode.javacv.cpp.opencv_core.*;
import com.googlecode.javacv.cpp.opencv_calib3d.*;
import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_video;
import java.awt.Color;
import java.awt.Graphics;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.event.MouseMotionListener;
import java.util.Random;
import javax.swing.JFrame;
import javax.swing.JPanel;

/** Interactive Surface FYP- 25 Interactive Surface FYP- 25 Interactive Surface FYP- 25
 *  General Information about this class -
 *  Comments -
 */
public class KalmanPoints extends Thread {

    CvKalman kf = opencv_video.cvCreateKalman(4, 2, 0);
    CvMat measurement = CvMat.create(2, 1, 1111638021);
    CvMat state = CvMat.create(4, 1, 1111638021);
    CvMat pronoise = CvMat.create(4, 1, 1111638021);
    int count = 0;
    int start = 1;

    @Override
    public void run() {
        vacate();
    }

    {
        vacate();
    }
    CvPoint lastPoint = cvPoint(0, 0);
    CvPoint lastKalman = cvPoint(0, 0);

    public void vacate() {
        int i = 0;
        CvPoint po = cvPoint(0, 0);

        CvMat trm = CvMat.create(4, 4, 1111638021);
        trm.put(0, 0, 1);
        trm.put(0, 1, 0);
        trm.put(0, 2, 1);
        trm.put(0, 3, 0);

        trm.put(1, 0, 0);
        trm.put(1, 1, 1);
        trm.put(1, 2, 0);
        trm.put(0, 3, 1);

        trm.put(2, 0, 0);
        trm.put(2, 1, 0);
        trm.put(2, 2, 1);
        trm.put(2, 3, 0);

        trm.put(3, 0, 0);
        trm.put(3, 1, 0);
        trm.put(3, 2, 0);
        trm.put(3, 3, 1);
        kf.transition_matrix(trm);

        CvMat ms = CvMat.create(4, 2, 1111638021);
        ms.put(0, 0, 1);
        ms.put(0, 1, 0);
        ms.put(0, 2, 0);
        ms.put(0, 3, 0);

        ms.put(1, 0, 0);
        ms.put(1, 1, 1);
        ms.put(1, 2, 0);
        ms.put(1, 3, 0);
        //kf.measurement_matrix(ms);
        cvSetIdentity(state, 0);
        cvSetIdentity(kf.measurement_matrix());
        //cvSetIdentity(kf.process_noise_cov(), 1e-2);
        //cvSetIdentity(kf.measurement_noise_cov(), 1e-2);
        cvSetIdentity(kf.process_noise_cov(), 1e-20);
        cvSetIdentity(kf.measurement_noise_cov(), 1e-20);
        cvSetIdentity(kf.error_cov_post(), .1);
        cvRange(kf.state_post(), 0, 0.1);
    }

    public static void main(String[] args) {
        ShashKalmanTest tr = new ShashKalmanTest();
        tr.start();
//        try {
//            Thread.sleep(1000000);
//        } catch (InterruptedException ex) {
//            Logger.getLogger(ShashKalmanTest.class.getName()).log(Level.SEVERE, null, ex);
//        }
    }

    public void restart() {
        kf = opencv_video.cvCreateKalman(4, 2, 0);
        measurement = CvMat.create(2, 1, 1111638021);
        state = CvMat.create(4, 1, 1111638021);
        pronoise = CvMat.create(4, 1, 1111638021);
        vacate();
        //System.gc();
    }

    public int[] kalman(int[] meas) {
        int[] guess = new int[2];
        Random rt = new Random();
        //CvPoint po = cvPoint(e.getX()+(int)(rt.nextGaussian()*20), e.getY()+(int)(rt.nextGaussian()*20));
        CvPoint po = cvPoint(meas[0], meas[1]);
        CvPoint real = cvPoint(meas[0], meas[1]);
        count++;
        if (start == 1) {
            start = 1;
            kf.state_pre().put(0, 0, po.x());
            kf.state_pre().put(1, 0, po.y());
            kf.state_pre().put(2, 0, 0);
            kf.state_pre().put(3, 0, 0);
        } else {
            lastPoint = real;
        }
        CvMat prediction =CvMat.create(4, 1, 1111638021);
        prediction =opencv_video.cvKalmanPredict(kf, null);
        System.out.println("predicted");
        measurement.put(0, 0, po.x());
        measurement.put(1, 0, po.y());

        cvMatMulAdd(kf.measurement_matrix(), state, measurement, measurement);
        CvMat estimated = opencv_video.cvKalmanCorrect(kf, measurement);
        System.out.println(count + " estimated= (" + estimated.get(0, 0) + "," + estimated.get(1, 0) + ")");
        cvRange(pronoise, 0, Math.sqrt(kf.process_noise_cov().get(0, 0)));
        cvMatMul(kf.transition_matrix(), state, state);
        cvAdd(state, pronoise, state, null);
        CvPoint KalPo = cvPoint((int) estimated.get(0, 0), (int) estimated.get(1, 0));

        if (start == 1) {
            start = 0;
        } else {
            lastKalman = KalPo;
        }
        guess[0] = (int) estimated.get(0, 0);
        guess[1] = (int) estimated.get(1, 0);
        return guess;
    }
}
